import matplotlib.pyplot as plt
import numpy as np

# 八个离散方向：右、上、左、下、右上、右下、左上、左下
directions = [
    (20, 0),
    (0, 20),
    (-20, 0),
    (0, -20),
    (20, 20),
    (20, -20),
    (-20, 20),
    (-20, -20),
]


def distance(p1, p2):
    """计算两点之间的欧几里得距离"""
    return np.sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)


def find_next_move(current, goal):
    """根据目标点选择最佳移动方向"""
    best_move = None
    min_distance = float("inf")

    for direction in directions:
        next_pos = (current[0] + direction[0], current[1] + direction[1])
        dist = distance(next_pos, goal)
        if dist < min_distance:
            min_distance = dist
            best_move = next_pos

    return best_move


def generate_path(start, goal):
    """生成从起点到终点的路径"""
    current = start
    path = [current]

    while not np.array_equal(current, goal):
        current = find_next_move(current, goal)
        path.append(current)

    return np.array(path)


def fix_path(path, path_point_list, end_idx):
    path_new = []
    end_point = path[0]
    for i, item in enumerate(path_point_list):
        item[0] += 200
        item[1] += 400
        path_new.extend(generate_path(end_point, item))
        end_point = item
    path_new.extend(generate_path(end_point, path[end_idx]))
    path_new.extend(path[end_idx:])
    return path_new


# 假设文件路径为 'data.npy'
pathnum = 1
current_target_index = []
data = np.load("path_dir/path" + str(pathnum) + ".npy", allow_pickle=True).tolist()
# 查看数据的形状以了解数据的结构
colors = ["r", "g", "b", "c", "m", "y", "k", "w"]
# 绘制数据
plt.figure(figsize=(8, 6))
plt.ioff()
for i in range(len(data)):
    path_x, path_y = zip(*data[i])
    path_x = [x - 200 for x in path_x]
    path_y = [y - 400 for y in path_y]
    # 标注颜色
    plt.plot(path_x, path_y, colors[i], label="期望路径")  #
    plt.scatter(path_x, path_y, color="red")  # 使用红色标记每个点


plt.xlabel("X-axis")
plt.ylabel("Y-axis")


# 显示绘图
plt.show()
# 示例起点和终点
######

data[1] = fix_path(data[1], [[30, -10]], 100)
data[0] = fix_path(data[0], [[30, -70], [90, -70]], 12)
data[3] = fix_path(data[3], [[130, 70], [330, -10]], 100)
data[5] = fix_path(data[5], [[210, 110], [110, 110], [110, -30], [390, -30]], 50)
#########
for i in range(len(data)):
    path_x, path_y = zip(*data[i])
    path_x = [x - 200 for x in path_x]
    path_y = [y - 400 for y in path_y]
    # 标注颜色和点的形状

    plt.plot(path_x, path_y, colors[i], label="期望路径")  #
    # 标记出每个点
    plt.scatter(path_x, path_y, color="red")  # 使用红色标记每个点
# 显示绘图
plt.show()
np.save("path_dir/pathX" + str(pathnum) + ".npy", data)
